#include "static_layer.h"

namespace costmap_2d{

StaticLayer::StaticLayer(boost::shared_ptr<Costmap> realtime_costmap_ptr, tf2_ros::Buffer* tf_buffer)
    :originCostmapPtr_(nullptr),realtimeCostmapPtr_(realtime_costmap_ptr),tfBuffer_(tf_buffer){
        
}

// 设置地图原始数据
void StaticLayer::setMap(boost::shared_ptr<Costmap> origin_costmap_ptr){
    originCostmapPtr_ = origin_costmap_ptr;
}

// 更新代价地图
void StaticLayer::updateCostmap(bool is_local){
    // 校验原始地图数据是否已经设置
    if(!originCostmapPtr_){
        LOG(WARNING) << "[StaticLayer::updateCostmap] originCostmapPtr_ is nullptr!" << endl;
        return;
    }
    if(!realtimeCostmapPtr_){
        LOG(WARNING) << "[StaticLayer::updateCostmap] realtimeCostmapPtr_ is nullptr!" << endl;
        return;
    }

    // 更新realtimeCostmapPtr_数据
    if(!is_local){      //全局代价地图
        // cout << "[StaticLayer::updateCostmap]  realtimeCostmapPtr_->frameId " << realtimeCostmapPtr_->frameId << endl;
        // 更新地图元数据
        realtimeCostmapPtr_->map_metadata = originCostmapPtr_->map_metadata;
        realtimeCostmapPtr_->costMap.clear();
        realtimeCostmapPtr_->costMap.resize(originCostmapPtr_->costMap.size(), costmap_2d::FREE_SPACE);
        cout << "[StaticLayer::updateCostmap]  obstacleCells_ size " << obstacleCells_.size() << endl;
        // 更新障碍物缓存数据
        if(obstacleCells_.empty()){
            for(int i=0; i<originCostmapPtr_->costMap.size(); ++i){
                if(originCostmapPtr_->costMap[i] == costmap_2d::LETHAL_OBSTACLE){  
                    // 填充障碍物 
                    GridCell grid_cell;
                    grid_cell.index = i;
                    realtimeCostmapPtr_->indexToMap(grid_cell.index, grid_cell.x, grid_cell.y);
                    grid_cell.sx = grid_cell.x;
                    grid_cell.sy = grid_cell.y;
                    grid_cell.cost = costmap_2d::LETHAL_OBSTACLE;
                    obstacleCells_[i] = grid_cell;
                }
            }
        }
        else{   //使用缓存数据
            for(auto& cell : obstacleCells_){
                int index = cell.second.index;
                realtimeCostmapPtr_->costMap[index] = costmap_2d::LETHAL_OBSTACLE;  
            }
        }
    }
    else{   //局部代价地图
        obstacleCells_.clear();
        realtimeCostmapPtr_->costMap.clear();
        realtimeCostmapPtr_->costMap.resize(realtimeCostmapPtr_->map_metadata.width * realtimeCostmapPtr_->map_metadata.height, costmap_2d::FREE_SPACE);

        // 变换到全局坐标
        // 获取机器人位姿
        geometry_msgs::TransformStamped transform;
        try{
            transform = tfBuffer_->lookupTransform("map", realtimeCostmapPtr_->frameId, ros::Time(0));
        }
        catch (tf2::TransformException ex){
            ROS_ERROR("[StaticLayer::updateCostmap] error: %s", ex.what());
            return;
        }
        tf2::Transform tf2_transform;
        tf2::convert(transform.transform, tf2_transform);     
        
        // cout << "tf2_transform: ..................................." << endl;
        for (int i = 0; i < realtimeCostmapPtr_->map_metadata.width; ++i){
            for (int j = 0; j < realtimeCostmapPtr_->map_metadata.height; ++j){
                int index = realtimeCostmapPtr_->mapToIndex(i, j);

                // 局部世界坐标 转 全局世界坐标
                double wx, wy;  //局部坐标
                realtimeCostmapPtr_->mapToWorld(i, j, wx, wy);
                tf2::Vector3 p(wx, wy, 0);  //全局坐标
                p = tf2_transform * p;

                // 全局地图获取代价值
                int mx, my;
                originCostmapPtr_->worldToMap(p.x(), p.y(), mx, my);

                // bool state = originCostmapPtr_->isValid(mx, my);
                // cout << "i: " << i << " j: " << j <<  " index " << index
                //     << " wx: " << wx << " wy: " << wy 
                //     << " p.x(): " << p.x() << " p.y(): " << p.y()
                //     << " mx: " << mx << " my: " << my << " state " << state << endl;

                // 校验数据是否有效
                if(!originCostmapPtr_->isValid(mx, my)){
                    realtimeCostmapPtr_->costMap[index] = costmap_2d::UNKNOWN;
                    continue;
                }
                else{
                    unsigned char cost = originCostmapPtr_->getCost(mx, my);
                    realtimeCostmapPtr_->costMap[index] = cost;

                    // 填充障碍物 
                    if(cost== costmap_2d::LETHAL_OBSTACLE){
                        GridCell grid_cell;
                        grid_cell.index = index;
                        grid_cell.x = i;
                        grid_cell.y = j;
                        grid_cell.sx = i;
                        grid_cell.sy = j;
                        grid_cell.cost = costmap_2d::LETHAL_OBSTACLE;
                        obstacleCells_[index] = grid_cell;
                    }
                }
            }
        }        
    }
}



// 清空代价地图
void StaticLayer::clearCostmap(){
    obstacleCells_.clear();
    originCostmapPtr_ = nullptr;
}

}; //costmap_2d